/* ============================================================
 * Part of 'Autostop' project
 * Under GPL license - http://www.gnu.org/licenses/gpl.html
 * (c) 2006 - Autostop team
 */

///@file process.c analyze process rutines

#include <assert.h>			// uses assert()
#include <stdlib.h>			// uses malloc/free
#include <stdio.h>
#include <string.h>

#include "analyzer.h"		// main defiitions

// min/max defnitions. note - these aree gcc-only
#define min(i,j)	((i) < (j) ? (i) : (j))
#define max(i,j)	((i) > (j) ? (i) : (j))

// module locals
static process_params_t default_params();						///< returns default params
/// Process stage 1 - from regular image to segmented map
static void process_phase1( pprocess_t pprocess,  const pimage_t pimage, int delta_time );
/// Process stage 2 - from segmented image to groups
static void process_phase2( pprocess_t pprocess );
/// Process stage 3 - from groups to moving objects
static void process_phase3( pprocess_t pprocess, uint64_t timestamp );
/// Initializes group structure
static void init_group( group_t* pgroup );
/// Merges two groups
static void merge_groups( group_t* lower, group_t* higher );
/// Adds pixel to group
static void add_pixel_to_group( group_t* pgroup, int x, int y, pixel_t value );


// ============================================================
/// Creates and initializes process. Assign default values to process params
pprocess_t create_process()
{
	// alocate process struct
	pprocess_t pprocess = (pprocess_t)malloc( sizeof( process_t ) );

	// initialize data
	assert( pprocess );

	pprocess->params			= default_params();
	pprocess->pbackground		= NULL;
	pprocess->psegmented		= NULL;
	pprocess->pgroupmap			= NULL;
	pprocess->processed_frames	= 0;
	pprocess->group_number		= 0;

	// objects
	pprocess->object_number		= 0;
	memset( pprocess->objects, 0, sizeof( pprocess->objects ) );

	// reference distance
	pprocess->pdistance_markers			= NULL;
	pprocess->reference_distance			= 0;

	return pprocess;
}

// ============================================================
/// Frees process. Does nothing if pprocess is NULL
///@param pprocess process to free
void free_process( pprocess_t pprocess )
{
	if( pprocess ) {
	
		// free sub-structures
		free_image( pprocess->pbackground );
		free_image( pprocess->psegmented );

		free_map( pprocess->pgroupmap );
		
		// free user-supplied reference distances
		free_image( pprocess->pdistance_markers );
		
		// free master struct
		free( pprocess );

	
	}
}

// ============================================================
/// Returns default process parameters
static process_params_t default_params()
{
	process_params_t params;
	params.background_factor		= 0.02*0.05;	// 1/50 blend in 1/20 sek
	params.foreground_treshold		= 85;
	params.segmentation_scale_log	= 3;	// real scale - 8
	params.segment_treshold			= 16;	// 32/255
	params.max_group_size			= 256;	// [px]
	params.min_group_size			= 32;	// [px]
	params.max_group_distance		= 640;	// [px/s]

	return params;
} 

// ============================================================
/// Adds image to running process. The image is processed, and process results may
/// be read from process then.
///\par
/// First image added to process determines image size. All subsequent image have to
/// be the same size.
///\param pprocess	running process. Must be a valid pointer
///\param pimage	new image. Must be a valid pointer.
///\param timestamp	image timestamp [ms]
///\note this function is time critical
void add_frame( pprocess_t pprocess, const pimage_t pimage, uint64_t timestamp )
{
	assert( pprocess );
	assert( pimage );

	// first image
	if ( pprocess->processed_frames == 0 ) {
		assert( ! pprocess->pbackground );
		
		// intial calculations
		pprocess->_int_background_factor = (int)( (1<<16) * pprocess->params.background_factor );
		// create background - copy
		pprocess->pbackground = create_image_copy( pimage );
		// create empty segmented image
		pprocess->psegmented = create_image
			( pimage->width		>> pprocess->params.segmentation_scale_log
			, pimage->height	>> pprocess->params.segmentation_scale_log
			);
		// create array for group indexes
		pprocess->pgroupmap = create_map( pprocess->psegmented->width, pprocess->psegmented->height );
		
	}
	// subsequent images
	else {
		assert( pprocess->pbackground );
		assert( pimage->width = pprocess->pbackground->width );
		assert( pimage->height = pprocess->pbackground->height );

		// calculate timespan
		int delta_time = timestamp - pprocess->last_frame_time;

		// phase 1 - get segmented image
		process_phase1( pprocess, pimage, delta_time );
		// phase 2 - get groups
		process_phase2( pprocess );
		// phase 3 - update objects with groups
		process_phase3( pprocess, timestamp );
	}

	// store timestamp and increment counter
	pprocess->last_frame_time = timestamp;
	pprocess->processed_frames ++;
}

// ============================================================
/// Processes incoming image:
///	- updates background storeed in process context
///	- creates segemented image in process context
///\param	pprocess		- process context
///\param	pimage		- incoming frame
///\param	delta_time	- time from last background update [ms]
///\note	This function is time-critical
static void process_phase1( pprocess_t pprocess,  const pimage_t pimage, int delta_time )
{
	assert( pprocess );
	assert( pimage );
	assert( pprocess->pbackground );
	assert( pprocess->psegmented );
	assert( pimage->width = pprocess->pbackground->width );
	assert( pimage->height = pprocess->pbackground->height );

	// clear segmented image
	foreach_pixel( psegmented_pixel, pprocess->psegmented ) *psegmented_pixel = 0;

	// calculate some values
	int bgfactor = pprocess->_int_background_factor * delta_time; // shifted by PRECISION_BITS bits!

	// process pixels
	pixel_t* pimage_pixel = pimage->pixels;
	pixel_t* pbkgnd_pixel = pprocess->pbackground->pixels;
	register int x,y; 
	for( y = 0; y < pimage->height; y++ )
	{
		for( x = 0; x < pimage->width; x++ )
		{
			// process:

			// update background
			*pbkgnd_pixel = (uint8_t) ( ( *pbkgnd_pixel * ( (1 << PRECISION_BITS) - bgfactor )
				+ *pimage_pixel * bgfactor ) >> PRECISION_BITS );

			// check if pixel's diff from background is above treshold
			if ( abs( *pimage_pixel - *pbkgnd_pixel ) > pprocess->params.foreground_treshold )
			{
				// increment segmented image
				image_pixel
					( pprocess->psegmented
					, x >> pprocess->params.segmentation_scale_log
					, y >> pprocess->params.segmentation_scale_log
					) += ( 1 << ( 8 - 2 * pprocess->params.segmentation_scale_log ) );
					// 8 -max bits in pixel minus pixel number (2^(log*2))
			}
			
			// increment poniters
			++ pimage_pixel;
			++ pbkgnd_pixel;
		}
	}
}

// ============================================================
/// Second stage processing:
///	- changes segmented image into groups. Warning - some groupos will have zero-pixels.
///\param pprocess		- process context
///\note	This function is time-critical, but operates on smaller amount of data than phase 1
static void process_phase2( pprocess_t pprocess )
{
	assert( pprocess );
	assert( pprocess->psegmented );
	assert( pprocess->pgroupmap );
	
	// groups joints array
	// this array is in fact a triange array (lower-right half is used).
	// Normally it stores 0, and if groups i and j are conencted, it has 1 in [ min(i,j) ][ max(i,j) ]
	// The array is uninitialized, and each row will be initializwd when new group will bedetected.
	int joints[MAX_GROUPS][MAX_GROUPS];

	// clear groups array
	pprocess->group_number = 0;

	// cache some data
	const int w = pprocess->psegmented->width;
	const int h = pprocess->psegmented->height;
	
	// go and seek!

	// phase 1 - create initial groups map and groups list
	int x, y, i, j; // indexes
	int*		pmap	= pprocess->pgroupmap->pixels;
	pixel_t*	ppixel	= pprocess->psegmented->pixels;
	for( y = 0; y < h; y++ ) {
		for( x=0; x < w; x++ ) {
			if ( *ppixel > pprocess->params.segment_treshold ) {
				int groupabove	= -1;	// group index of pixel above
				int groupbefore	= -1;	// group index of pixel at left
				int currentgroup= -1;	// which group we will choose for current pixel
				
				// find group above
				if ( y > 0 ) {
					// first - look at top-right pixel
					if ( x < w-1 ){
						groupabove = *(pmap-w+1);
					}
					// second - look at pixel at top
					if ( groupabove < 0 ) {
						groupabove = *(pmap-w);
					}
					// lastly - look at pixel at top-left
					if ( groupabove < 0 && x > 0 ) {
						groupabove = *(pmap-w-1);
					}
				}
				// find group at left
				if ( x > 0 ) {
					groupbefore = *(pmap-1);
				}
	
				// if group above exists, take it
				if ( groupabove >= 0 ) {
					currentgroup = groupabove;
					if ( groupbefore >= 0 && groupbefore != groupabove ){
						// connect groups
						joints[ min( groupbefore, groupabove ) ][ max(groupbefore, groupabove ) ] = 1;
					}
				// if there is a group before, take it
				} else if ( groupbefore >= 0 ) {
					currentgroup = groupbefore;
				}
				// if there is a room for motre groups, create new group
				else if ( pprocess->group_number < MAX_GROUPS ) {
					// create new group;
					currentgroup = pprocess->group_number++;
					init_group( pprocess->groups + currentgroup );
					// prepare joints
					for( i = 0; i < pprocess->group_number; i++ ) {
						joints[i][pprocess->group_number-1] = 0;
					}
					
				}
				// no room for more groups, leave pixel ungrouped :(
				
				// assign group to pixel
				*pmap = currentgroup;

				// add pixel to group
				if ( currentgroup >= 0 ){
					add_pixel_to_group( pprocess->groups + currentgroup, x, y, *ppixel );
				}

			} // pixel above treshold
			else {
				*pmap = -1; // ignore
			}

			// increment pointers
			pmap ++;
			ppixel ++;
		}
	}

	// phase 2 - merge groups
	// iterate groups, merge higher-numner groups with lower number groups.
	// iterate backwards - so data is cumulated in lower-number groups
	for( i = pprocess->group_number - 1 ; i >= 1;  i-- ) {	// from last to 2nd group
		int ac = -1;										// index of last group we were merged with
		for( j = i - 1; j >= 0; j-- ) {						 // from current higher group (i) to first group
			if ( joints[j][i] ){ // j < i, always
				merge_groups( pprocess->groups + j, pprocess->groups + i );
				if ( ac >= 0 ) {
					// make sure that 'j' will be conected with 'ac'
					joints[j][ac] = 1;
				}
				ac = j;
				//printf("frame %d, join %d<-%d\n", pprocess->processed_frames, j, i); // TODO remove debug
			}
		}
	}

}

// ============================================================
/// Initializes group structure.
///\par	pgroup	- pointer to group object. must be valid
///\todo maybe create public?
static void init_group( group_t* pgroup )
{
	assert( pgroup );

	// statistics
	pgroup->mx = 0;
	pgroup->my = 0;

	pgroup->minx = 99999;	// TODO or another reasonable max value here, or image width/height
	pgroup->miny = 99999;
	pgroup->maxx = 0;
	pgroup->maxy = 0;

	// counters
	pgroup->value = 0;

	pgroup->pixels = 0;
	pgroup->xsum = 0;
	pgroup->ysum = 0;

}

// ============================================================
/// Adds counters of higher group to lower, leaving counters in higher at zero
///\par	lower	- pointer to group object. must be valid.
///\par	higher	- pointer to group object. must be valid.
static void merge_groups( group_t* lower, group_t* higher )
{
	// add counters
	lower->pixels += higher->pixels;
	lower->xsum += higher->xsum; 
	lower->ysum += higher->ysum;
	lower->value += higher->value;

	// update max/mins
	lower->maxx = max( lower->maxx, higher->maxx );
	lower->maxy = max( lower->maxy, higher->maxy );
	lower->minx = min( lower->minx, higher->minx );
	lower->miny = min( lower->miny, higher->miny );

	// reset counters
	higher->value = 0;

	higher->pixels = 0;
	higher->xsum = 0;
	higher->ysum = 0;
}

// ============================================================
/// Adds pixel to group - updates max/min values, adds pixel value
///\par	pgroup	- pointer to group data. must be valid
///\par	x		- pixel coord
///\par y		- pixel coord
///\par value	- pixel value
static void add_pixel_to_group( group_t* pgroup, int x, int y, pixel_t value )
{
	assert( pgroup );
	
	// increment counters
	pgroup->xsum += x * value;
	pgroup->ysum += y * value;
	pgroup->pixels++;
	pgroup->value += value;

	// update boundaries
	pgroup->maxx = max( pgroup->maxx, x );
	pgroup->maxy = max( pgroup->maxy, y );
	pgroup->minx = min( pgroup->minx, x );
	pgroup->miny = min( pgroup->miny, y );
}

// ============================================================
/// Checks if group is interesting - if it is not empty, not too small or to big (accroding to process params)
///\par pgroup		- pointer to group data, Must be valid
///\par	pprocess	- pointer to process data. Must be valid
///\return	0 if not interesting, 1 otherwise
static int is_interesting_group( group_t* pgroup, pprocess_t pprocess )
{
	assert( pprocess );
	assert( pgroup );

	// cache some data:
	// groups siez in segemted pixels (as in group data)
	const int min_group_size = pprocess->params.min_group_size >> pprocess->params.segmentation_scale_log;
	const int max_group_size = pprocess->params.max_group_size >> pprocess->params.segmentation_scale_log;

	// 1. check if group wasn't merged - pixels > 0
	if ( pgroup->pixels == 0 )
		return 0; // go away, you don't exists

	// 2. check group min size
	if 	(
			( pgroup->maxx - pgroup->minx ) < min_group_size
			||
			( pgroup->maxy - pgroup->miny ) < min_group_size
		)
		return 0; // go away, you are too small!

	// 3. check if group max size
	if 	(
			( pgroup->maxx - pgroup->minx ) > max_group_size
			||
			( pgroup->maxy - pgroup->miny ) > max_group_size
		)
		return 0; // go away, you are too big!


	return 1; // success!
	
}

// ============================================================
/// Copies group data into object point. Uses process settings to determine group scaling factor,
///	and suppied timestamp
///\par	pprocess		- pointer to process data. Must be valid
///\par	pgroup			- pointer to source group data. Must be valid
///\par	ppoint (OUT)	- pointer to target points data. Must be valid
///\par timestamp		- point timestamp
static void group_to_point( pprocess_t pprocess, group_t* pgroup, object_point_t* ppoint, uint64_t timestamp )
{
	assert(pprocess);
	assert( ppoint );
	assert( pgroup );
	
	int factor = 1 << pprocess->params.segmentation_scale_log;

	ppoint->x = (int) pgroup->mx * factor;
	ppoint->y = (int) pgroup->my * factor;
	ppoint->timestamp = timestamp;

	// if marker set, get its data
	if ( pprocess->pdistance_markers )
	{
		ppoint->marker = image_pixel( pprocess->pdistance_markers, ppoint->x, ppoint->y );
	}
	else
	{
		ppoint->marker = -1;
	}
}

// ============================================================
/// Adds group to object
void add_group_to_object( pprocess_t pprocess, object_t* pobject, group_t* pgroup, uint64_t timestamp )
{
	assert( pprocess );
	assert( pobject );
	assert( pgroup );
	
	group_to_point( pprocess, pgroup, pobject->points + pobject->point_number, timestamp );

	// check if marker was passed
	if ( pobject->point_number > 1 )
	{
		int currentMarker	= ( pobject->points + pobject->point_number )->marker;
		int lastMarker		= ( pobject->points + pobject->point_number - 1 )->marker;

		// cache marker crossing times
		if ( lastMarker == 0 && currentMarker == 1 )
		{
			pobject->marker01Time = timestamp;
		}
		else if ( lastMarker == 1 && currentMarker == 2 )
		{
			pobject->marker12Time = timestamp;
			// try calculate speed
			if ( pobject->marker01Time > 0 )
			{
				pobject->speed = (double)pprocess->reference_distance / (double)( pobject->marker12Time - pobject->marker01Time );
			}
		}
		
	}
	

	pobject->point_number ++;
}

// ============================================================
/// Adds new obejct to process
///\return pointer to added object, or NULL on error
object_t* add_new_object( pprocess_t pprocess )
{
	assert( pprocess );

	if ( pprocess->object_number < MAX_OBJECTS )
	{
		object_t* pobject = pprocess->objects + pprocess->object_number;
		// initialize object
		pobject->point_number 	= 0;
		pobject->marker01Time	= 0;
		pobject->marker12Time	= 0;
		pobject->speed			= 0.0;
		
		pprocess->object_number ++;

		return pobject;
	}

	return NULL;
}

// ============================================================
/// Analyzes list of groups in process, and updates list of moving objects accordingly.
///\par	pprocess	- pointer to process data. Must be valid
///\par timestamp	- timestamp of currently processed frame
static void process_phase3( pprocess_t pprocess, uint64_t timestamp )
{
	assert( pprocess );

	// for each group
	int group, object; // indexes
	for( group = 0; group < pprocess->group_number; group++ )
	{
		int closest_object = -1;	// object closest to group
		int closest_distance = -1;	// its distance
		
		// cache some data
		group_t* pgroup = pprocess->groups + group;

		// check if group applies
		if ( ! is_interesting_group( pgroup, pprocess ) )
			continue;

		// calculate group mean poxition
		pgroup->mx = pgroup->xsum / pgroup->value;
		pgroup->my = pgroup->ysum / pgroup->value;
		
		// find group precedesor
		for( object = 0; object < pprocess->object_number; object++ )
		{
			// cache data
			object_t* pobject = pprocess->objects + object;

			// is valid?
			if ( pobject->point_number != 0 )
			{
				// time difference from group to object
				int delta_time = timestamp - pobject->points[pobject->point_number-1].timestamp;
				// spatial distance
				int dx = pgroup->mx - pobject->points[pobject->point_number-1].x;
				int dy = pgroup->my - pobject->points[pobject->point_number-1].y;
				int distance = abs(dx) + abs(dy);	// 1st module of diff vector

				// is distance within limit?
				if ( pprocess->params.max_group_distance * delta_time < distance * 1000 )
				{
					// is distance smaller than mnimum?
					if ( distance < closest_distance || closest_distance < 0 )
					{
						closest_distance = distance;
						closest_object = object;
					}
				}
			}
		}

		// add group to closest_object, if exists
		if ( closest_object >= 0)
		{
			// cache
			object_t* pobject = pprocess->objects + closest_object;

			// object should be valid
			assert( pobject->point_number );
			
			// if the object was already updated in this frame
			if ( pobject->points[pobject->point_number-1].timestamp == timestamp )
			{
				// TODO clone (fork) object here
			}
			// update object, if there is a room for more points, and non static
			else if ( pobject->point_number < MAX_POINTS && closest_distance > 0  )
			{
				add_group_to_object( pprocess, pobject, pgroup, timestamp );
			}
		}
		// create new object, if there is a room
		else if ( pprocess->object_number < MAX_OBJECTS )
		{
			object_t* pobject	= add_new_object( pprocess );
			if ( pobject )
			{
				add_group_to_object( pprocess, pobject, pgroup, timestamp );
			}
		}
		
	}
	
}

// ============================================================
/// Removes object from object list, reaaranging object list accordingly.
///\par	pprocess		- pointer to process data. Must be valid
///\par object_index	- ordinal index of object to be removed
void remove_object( pprocess_t pprocess, int object_index )
{
	assert( pprocess );
	assert( object_index >=0 && object_index < pprocess->object_number );

	// if only one object - just forget about it
	if ( pprocess->object_number == 1 )
	{
		pprocess->object_number = 0;
	}
	// swap last object with removed, decrease object counter
	else
	{
		memcpy( pprocess->objects + object_index,
			 pprocess->objects + pprocess->object_number -1,
			 sizeof( object_t )
			 );
		pprocess->object_number --;
	}
}

// eof

